package robocup.component.worldobjects;

import java.util.LinkedList;

import robocup.component.geometry.Circle;
import robocup.component.geometry.LineSegment;
import robocup.component.geometry.StraightLine;
import robocup.component.geometry.Vektor;

/**
 * A line marks the border of the field.
 */
public class Line {

  private final Vektor dummyVektor1 = Vektor.getDummyInstance();
  private final Vektor dummyVektor2 = Vektor.getDummyInstance();
  
  private final Vektor emptyVektor = new Vektor(Vektor.XY, java.lang.Double.MAX_VALUE, java.lang.Double.MAX_VALUE);
  
  /**
   * @return returns a new Line-Object
   */
  public final static Line getDummyLine() {

    return new Line(new Vektor(0, 0), new Vektor(1, 90));
  }

  /**
   * The cycle, in which the line was seen last.
   */
  private int    cycle;

  /**
   * The unique id of the line.
   */
  private int    id;

  /**
   * The first point defining the line.
   */
  private final Vektor point1 = Vektor.getDummyInstance();

  /**
   * The second point defining the line.
   */
  private final Vektor point2 = Vektor.getDummyInstance();

  /**
   * Line defined by 2 points.
   * 
   * @param iPoint1
   * @param iPoint2
   */
  public Line(
      final Vektor iPoint1,
      final Vektor iPoint2) {

    this.point1.copy(iPoint1);
    this.point2.copy(iPoint2);
  }

  /**
   * Fieldline
   * 
   * @param iCycle -
   *          time, when seen last
   * @param iId -
   *          the line id
   * @param iPoint1 -
   *          start-point of line
   * @param iPoint2 -
   *          end-point of line
   */
  public Line(
      final int iCycle,
      final int iId,
      final Vektor iPoint1,
      final Vektor iPoint2) {

    this.id = iId;
    this.cycle = iCycle;
    this.point1.copy(iPoint1);
    this.point2.copy(iPoint2);
  } // end of constructor Line(int iCycle, int iId, Vektor iPoint1, Vektor

  /**
   * The length of a line as distance between the 2 line points.
   * 
   * @return the length
   */
  public double getLength() {

    return this.point1.sub(this.point2).getLength();
  }

  /**
   * @param l
   *          the line to intersect with
   * @return the intersectionpoint of this line with l
   */
  public Vektor getIntersectionPointWithLine(final Line l) {

    final Vektor p1 = this.point1;
    final Vektor p2 = this.point2;
    final Vektor p3 = l.point1;
    final Vektor p4 = l.point2;
    double m1 = 0;
    double m2 = 0;
    double n1 = 0;
    double n2 = 0;
    double x = 0;
    double y = 0;
    if (p1.x == p2.x) {
      m1 = (p1.y - p2.y) / 0.01;
    }
    else {
      m1 = (p1.y - p2.y) / (p1.x - p2.x);
    }
    if (p3.x == p4.x) {
      m2 = (p3.y - p4.y) / 0.01;
    }
    else {
      m2 = (p3.y - p4.y) / (p3.x - p4.x);
    }
    n1 = p1.y - m1 * p1.x;
    n2 = p3.y - m2 * p3.x;
    if (m1 == m2) {
      x = (n1 - n2) / 0.01;
    }
    else {
      x = (n1 - n2) / (m2 - m1);
    }
    y = m1 * x + n1;
    return new Vektor(Vektor.XY, x, y);
  }

  /**
   * moves point 2 in that way, that the line has length l.
   * 
   * @param l
   *          makes the line having length l
   * 
   * DEPRECATED as it overcreates new Vektors (change this before usage)
   */
  @Deprecated
  public void setLength(final double l) {

    if (l <= 0) {
      System.out.println("[Line] length must be bigger than 0");
      return;
    }
    final Vektor actualDist = this.point2.sub(this.point1);
    this.point2.copy(this.point1.add(actualDist.setLength(l)));
  }

  /**
   * sets point1
   * 
   * @param p1
   *          the point to set
   * 
   */
  public void setPoint1(final Vektor p1) {

    this.point1.copy(p1);
  }

  /**
   * changes point1 and point2
   * 
   */
  public void changeDirection() {

    final Vektor buf = new Vektor();
    buf.copy(this.point2);
    this.point2.copy(this.point1);
    this.point1.copy(buf);
  }

  /**
   * sets point2
   * 
   * @param p2
   *          the point to set
   * 
   */
  public void setPoint2(final Vektor p2) {

    this.point2.copy(p2);
  }

  /**
   * copy point1
   * 
   * @param p1
   *          the value to copy
   * 
   */
  public void copyPoint1(final Vektor p1) {

    this.point1.copy(p1);
  }

  /**
   * @return the angle from point 1 to point 2
   */
  public double getAngle() {

    return this.point1.getAngleTo(this.point2);
  }

  /**
   * copy point2
   * 
   * @param p2
   *          the value to copy
   * 
   */
  public void copyPoint2(final Vektor p2) {

    this.point2.copy(p2);
  }

  /**
   * Gets the intersection points of the line and a circle.
   * 
   * @param c
   *          the circle to intersect with
   * 
   * @return the intersection points (0-2)
   */
  public LinkedList<Vektor> getIntersectionPoints(final Circle c) {

    return this.getPointsWithDistanceToPoint(c.getRadius(), c.getCenter());
  }

  /**
   * Gets the intersection points of the line and a circle.
   * 
   * @param distance
   *          The circle's radius.
   * @param point
   *          The circle's center.
   * @return points
   */
  public LinkedList<Vektor> getPointsWithDistanceToPoint(final double distance,
      final Vektor point) {

    final LinkedList<Vektor> points = new LinkedList<Vektor>();
    final Vektor p = this.getPointWithSmallestDistanceToPoint(point);
    final Vektor pointToP = p.sub(point);
    final double pointToPLength = pointToP.getLength();
    // no intersection
    if (pointToPLength > distance) {
      return points;
    }
    // tangent
    if (pointToPLength == distance) {
      points.add(p);
      return points;
    }
    // secant
    final double alpha = Math.asin(pointToPLength / distance);
    final double b = Math.cos(alpha) * distance;
    double anglePointToP = pointToP.getAngle();
    // a Vektor with length = 0 has no angle
    if (pointToP.getLength() == 0.0) {
      anglePointToP = 90.0 + this.point1.sub(this.point2).getAngle();
    }
    points.add(p.add(new Vektor(Vektor.POLAR, b, anglePointToP - 90)));
    points.add(p.add(new Vektor(Vektor.POLAR, b, anglePointToP + 90)));
    return points;
  }

  /**
   * Caluculates the point with the desired y-value.
   * 
   * @param y
   *          the desired y-value
   * @return returns the point on the line, where the y-value is y
   */
  public Vektor getPointWithYValue(final double y) {

    if (this.point1.y == this.point2.y) {
      return null;
    }
    final Vektor v = Vektor.getDummyInstance();
    this.getPointWithYValue(y, v);
    return v;
  }

  /**
   * Calculates the Point on the line with desired y-value. Copies the result to
   * v
   * 
   * @param y
   *          the desired y-value
   * @param v
   *          the container for result
   */
  public void getPointWithYValue(final double y,
      final Vektor v) {

    if (this.point1.y == this.point2.y) {
      v.setEmpty();
      return;
    }
    final double m = (this.point1.y - this.point2.y) / (this.point1.x - this.point2.x);
    final double n = this.point1.y - this.point1.x * m;
    v.x = (y - n) / m;
    v.y = y;
  }

  /**
   * Caluculates the point with the desired x-value.
   * 
   * @param x
   *          the desired x-value
   * @return returns the point on the line, where the x-value is x
   */
  public Vektor getPointWithXValue(final double x) {

    if (this.point1.x == this.point2.x) {
      return emptyVektor;
    }
    final Vektor v = Vektor.getDummyInstance();
    this.getPointWithXValue(x, v);
    return v;
  }

  /**
   * Calculates the Point on the line with desired x-value. Copies the result to
   * v
   * 
   * @param x
   *          the desired x-value
   * @param v
   *          the container for result
   */
  public void getPointWithXValue(final double x,
      final Vektor v) {

    if (this.point1.x == this.point2.x) {
      v.setEmpty();
      return;
    }
    final double m = (this.point1.y - this.point2.y) / (this.point1.x - this.point2.x);
    final double n = this.point1.y - this.point1.x * m;
    v.x = x;
    v.y = m * x + n;
  }

  /**
   * @param point -
   *          Vektor absolute position
   * @return Vektor - point on line with smallest distance to the specified
   *         point <code>point</code>. TODO test
   */
  public Vektor getPointWithSmallestDistanceToPoint(final Vektor point) {

    // lot will be the shortest vector from the point to the line
    // (or from the line to the point ....)
    final Vektor lot = this.point1.sub(this.point2);
    lot.rotate(90.0);
    lot.setLength(this.getDistanceToPoint(point));
    // there should be a formula that tell the direction of lot
    // but this should work as well .....
    if (this.getDistanceToPoint(lot.add(point)) > 0.001) {
      lot.rotate(180);
    }
    return lot.addToThis(point);
  }

  /**
   * @param point -
   *          Vektor absolute position
   * @return double - minimal distance from the specified point
   *         <code>point</code> to line TODO test
   */
  public double getDistanceToPoint(final Vektor point) {

    this.dummyVektor1.copy(point1).subFromThis(point);
    this.dummyVektor2.copy(point1).subFromThis(point2);
    return Math.abs(this.dummyVektor1.cross(this.dummyVektor2) / this.dummyVektor2.getLength());
  }

  /**
   * @param point
   * @return <code>true</code>, if the given point lies on the line,
   *         <code>false</code> otherwise.
   */
  public boolean isOnLine(final Vektor point) {

    return this.getDistanceToPoint(point) < 0.001;
  }

  /**
   * @return the perpendicular bisector of the side (Mittelsenkrechte)
   * 
   */
  public StraightLine getPerpendicularBisector() {

    return new StraightLine(this.point1.getVektorBetween(this.point2), this
        .getRightAngle());// toDegrees(getRectAngle() + 90));
  }

  /**
   * @return the absolute angle of the line
   */
  private double getRightAngle() {

    double divisor = this.getGradient();
    if (divisor == 0) {
      divisor = 0.00001; // Avoid dividing through zero
    }
    return Math.atan(-1 / this.getGradient());
  }

  /**
   * @return the gradient of the line. If the line is parallel to the y-Axis, a
   *         blur effect will be established.
   */
  private double getGradient() {

    double e = 0;
    if (this.point1.x == this.point2.x) {
      e = 0.00001; // Avoid dividing by 0
    }
    return (this.point2.y - this.point1.y) / (this.point2.x - this.point1.x + e);
  }

  // ************* Getters + Setters *************************

  /**
   * Returns the id of the line.
   * 
   * @return id
   */
  public int getId() {

    return this.id;
  } // end of method getId()

  /**
   * Sets the id of the line.
   * 
   * @param id
   *          the id to set
   */
  public void setId(final int id) {

    this.id = id;
  }

  /**
   * @return Returns the point1.
   */
  public final Vektor getPoint1() {

    return this.point1;
  }

  /**
   * @return Returns the point2.
   */
  public final Vektor getPoint2() {

    return this.point2;
  }

  /**
   * @return Returns the cycle.
   */
  public int getCycle() {

    return this.cycle;
  }

  /**
   * @param cycle
   *          The cycle to set.
   */
  public void setCycle(final int cycle) {

    this.cycle = cycle;
  }

  /**
   * (non-Javadoc)
   * 
   * @see java.lang.Object#toString()
   */
  @Override
  public String toString() {

    return this.point1 + " - " + this.point2;
  }
  
  /**
   * <br>
   * NEW OBJECTS: 2
   * 
   * @return a StraightLine that covers this line
   */
  public final StraightLine getStraightLine() {
    return new StraightLine(this.point1, this.point2);
  }

  
  /**
   * <br>
   * NEW OBJECTS: 4
   * 
   * @return a LineSegment that covers this line
   */
  public final LineSegment getLineSegment() {
    return new LineSegment(this.point1, this.point2);
  }

} // end of class Line
